#include "serial_imu.hpp"
#include <iostream>

/// @brief 
/// @param port 
/// @param baud 
serial_imu::serial_imu(std::string port, int baud)
     : Serial(port, baud)
{
    open();

    runThreadFunc();
   // startTimer(2);
}

void serial_imu::recvMsg(std::vector<u_int8_t> &msg)
{
    //接收的打印出来
    // std::stringstream ss;
    // for(auto i: msg)
    // {
    //     ss << toHex(i) << " ";
    // }
    //  std::cout << "recv: " << ss.str() << std::endl;
    // std::cout<<"recv0: "<<msg.size()<<std::endl;
   int size,vect,count;
   int seat=0;
   char ch_add;
   size=msg.size();
   while((seat+11)<=size)
   {
    if(msg[seat]==0x55)
    {
     
     ch_add=0;
     for(int i=0;i<10;i++)
     {
        ch_add+=msg[seat+i];
     }
     if(ch_add==msg[seat+10])
     {
        
        for(int j=0;j<11;j++)
           {
             rec_add_str.push_back(msg[j+seat]);
             
           }
        // display_rec_hex(rec_add_str);  
         //std::cout<<std::endl;  
         // if(msg[seat+1]==0x59)
         // std::cout<<"quat: "<<seat<<"size="<<size<<std::endl;    
        seat+=11;
     }
     else seat++;
     
    }
    else seat++;
     

   }
   if(seat>0)
   {
    auto it = msg.begin();
    for(int i=0;i<seat;i++)
      msg.erase(it);
   }
  // std::cout<<"recv: "<<msg.size()<<std::endl;
}
int serial_imu::rec_ok(std::vector<u_int8_t> &msg)
{
    // int flag=0;
    // u_int8_t ch=0;
    // if(msg[0]!=0xaa)return 1;
    // if(msg.size()!=12)return 2;
    // for(int i=0;i<11;i++)
    //     ch+=msg[i];
    // if(ch==msg[11])    
    //     return flag;
    //   else return 3; 
      return 0; 
}
void serial_imu::startTimer(int ms_s)
{
     interval = std::chrono::milliseconds(ms_s);   
     if (!running) {
            running = true;
            auto thread = std::thread([this]() {
                while (running) {
                                 
                   std::this_thread::sleep_for(interval);
                }
            });
            thread.detach(); // 让定时器在后台运行，防止主线程阻塞
        }
}


